#include <Kalman.h>
#include <stdint.h>

// 二维卡尔曼滤波
float Q_angle;
float Q_bias;
float R_measure;

static float angle;   // 角度
static float bias;    // 陀螺仪漂移
static float rate;    // 角速度
static float P[2][2]; // 误差协方差矩阵

void Kalman_Init(void)
{
    Q_angle = 0.02f; // 角度噪声协方差
    Q_bias = 0.06f;  // 陀螺仪漂移噪声协方差
    R_measure = 0.01f;       // 角度测量噪声

    angle = 0.0f; // 复位角度
    bias = 0.0f;  // 复位陀螺仪漂移

    P[0][0] = 0.0f; // 协方差矩阵
    P[0][1] = 0.0f;
    P[1][0] = 0.0f;
    P[1][1] = 0.0f;
}

// dt：周期
// Gyro:陀螺仪角速度值
// Accel：加速度计算出来的角度
float Kalman_getAngle(float dt, float Gyro_rate, float Acc_Angle)
{
#define radian_to_angle 57.295779513f

    float K[2];      // 卡尔曼增益
    float S;         // 计算卡尔曼增益时候的分母
    float angle_err; // 计算最优估计值时的角度误差

    /* Step 1 */
    // 基于系统的上一状态的预测值
    rate = Gyro_rate - bias;   // 陀螺仪：角速度 - 角速度漂移
    angle = angle + rate * dt; // 陀螺仪角速度积分成的角度=

    /* Step 2 */
    // 对应于X(k|k-1)的协方差更新
    P[0][0] += dt * (dt * P[1][1] - P[0][1] - P[1][0] + Q_angle);
    P[0][1] -= dt * P[1][1];
    P[1][0] -= dt * P[1][1];
    P[1][1] += Q_bias * dt;

    /* Step 3 */
    // 计算卡尔曼增益
    S = P[0][0] + R_measure; // 分母

    K[0] = P[0][0] / S;
    K[1] = P[1][0] / S;

    /* Step 4 */
    // 计算最优估计值
    angle_err = Acc_Angle - angle; // 角度误差

    angle += K[0] * angle_err;
    bias += K[1] * angle_err;

    /* Step 5 */
    // 更新k状态下X(k|k)的协方差
    P[0][0] -= K[0] * P[0][0];
    P[0][1] -= K[0] * P[0][1];
    P[1][0] -= K[1] * P[0][0];
    P[1][1] -= K[1] * P[0][1];
    return angle * radian_to_angle;
}

//һά�������˲�
//1. �ṹ�����Ͷ���


//2. �Ը߶�Ϊ�� ���忨�����ṹ�岢��ʼ������
//KFP KFP_height={0.02,0,0,0,0.001,0.543};
KFP KFP_TS0= {0.02,0,0,0,0.02,0.8};
KFP KFP_TS1= {0.02,0,0,0,0.02,0.8};
KFP KFP_TS2= {0.02,0,0,0,0.02,0.8};
KFP KFP_TS3= {0.02,0,0,0,0.05,0.6};
KFP KFP_TS4= {0.02,0,0,0,0.05,0.6};
KFP KFP_TS5= {0.02,0,0,0,0.05,0.6};
KFP KFP_TS6= {0.02,0,0,0,0.05,0.8};
KFP KFP_TS7 = {0.02, 0, 0, 0, 0.08, 0.543};
KFP KFP_TS8= {0.02,0,0,0,0.05,0.8};

/**
 *�������˲���
 *@param KFP *kfp �������ṹ�����
 *   float input ��Ҫ�˲��Ĳ����Ĳ���ֵ�����������Ĳɼ�ֵ��
 *@return �˲���Ĳ���������ֵ��
 */
float kalmanFilter(KFP *kfp,float input)
{
    //Ԥ��Э����̣�kʱ��ϵͳ����Э���� = k-1ʱ�̵�ϵͳЭ���� + ��������Э����
    kfp->Now_P = kfp->LastP + kfp->Q;
    //���������淽�̣����������� = kʱ��ϵͳ����Э���� / ��kʱ��ϵͳ����Э���� + �۲�����Э���
    kfp->Kg = kfp->Now_P / (kfp->Now_P + kfp->R);
    //��������ֵ���̣�kʱ��״̬����������ֵ = ״̬������Ԥ��ֵ + ���������� * ������ֵ - ״̬������Ԥ��ֵ��
    kfp->out = kfp->out + kfp->Kg * (input -kfp->out);//��Ϊ��һ�ε�Ԥ��ֵ������һ�ε����ֵ
    //����Э�����: ���ε�ϵͳЭ����� kfp->LastP ����һ������׼����
    kfp->LastP = (1-kfp->Kg) * kfp->Now_P;
    return kfp->out;
}

/**
 *���ÿ������˲��� ʵ��
 */
//int height;
//int kalman_height=0;
//kalman_height = kalmanFilter(&KFP_height,(float)height);




